﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Reinforcement;
using Probability;

namespace sau
{
    class AircraftModel : CCEnvironment
    {
        //state
        double alpha;
        double q;
        double theta;
        double desiredTheta;
        ASampler sampler = new ASampler();

        double time; //in steps (1 step = 20ms)
        int changeTime = 0;

        //h for RK4
        double h = 0.01;

        // used int GetRandomNumber() method
        private Random random;

        public AircraftModel()
        {
            random = new Random();
            CCStartInRandomState();
        }

        //  begin a run
        public void CCStartInRandomState()
        {
            time = 0;

            changeTime = (int) GetRandomNumber(1, 160); // 0 - 8 sec

            alpha = GetRandomNumber(-0.5, 0.5);
            q = GetRandomNumber(-1.4, 1.4);
            theta = GetRandomNumber(-0.5, 0.5);
        }

        // what is the state of the environment? state is for the decision maker, extension is e.g. for drawing
        public void CCGetCurrentState(ref double[] state,   // 0 - alpha, 1 - q, 2 - theta, 3 - desired theta 
            ref double[] state_extension)                   // state_extension is ignored
        {
            if (state != null)
            {
                state[0] = alpha;
                state[1] = q;
                state[2] = theta;
                state[3] = desiredTheta;
            }
        }

        // let's go 
        public void CCMove(double[] decision,   //  action(s) to be performed (in the same time of course)   
                           ref double reward)   //  reward why?
        {
            time = time + 1; //add 1 time step (20ms)

            //calculate new state
            double newAlpha = AlphaRungeKutta(decision[0]);
            double newQ = QRungeKutta(decision[0]);
            double newTheta = ThetaRungeKutta(decision[0]);

            //save new state as current
            alpha = newAlpha;
            q = newQ;
            theta = newTheta;

            NewDesiredTheta();

            //create state array - (MM) JAKI TO MA SENS?
            double[] state = new double[4];
            state[0] = alpha;
            state[1] = q;
            state[2] = theta;
            state[3] = desiredTheta;

            //TODO:: sendthis to advisor
            reward = CalculateReward();
        }

        private double GetNextAlpha(double alpha, double q, double delta)
        {
            return (-0.313 * alpha + 56.7 * q + 0.232 * delta);
        }

        private double GetNextQ(double alpha, double q, double delta)
        {
            return (-0.0139 * alpha - 0.426 * q + 0.0203 * delta);
        }

        private double GetNextTheta(double q)
        {
            return 56.7 * q;
        }

        private double AlphaRungeKutta(double delta)
        {
            double k1 = h * GetNextAlpha(alpha, q, delta);
            double k2 = h * GetNextAlpha(alpha + 0.5 * k1, q, delta);
            double k3 = h * GetNextAlpha(alpha + 0.5 * k2, q, delta);
            double k4 = h * GetNextAlpha(alpha + k3, q, delta);
            return alpha + (1 / 6.0) * (k1 + 2 * k2 + 2 * k3 + k4);
        }

        private double QRungeKutta(double delta)
        {
            double k1 = h * GetNextQ(alpha, q, delta);
            double k2 = h * GetNextQ(alpha, q + 0.5 * k1, delta);
            double k3 = h * GetNextQ(alpha, q + 0.5 * k2, delta);
            double k4 = h * GetNextQ(alpha, q + k3, delta);
            return q + (1 / 6.0) * (k1 + 2 * k2 + 2 * k3 + k4);
        }

        private double ThetaRungeKutta(double delta)
        {
            return (q + h * GetNextTheta(delta));
        }

        private void NewDesiredTheta()
        {
            if (time == changeTime)
            {
                //TODO:: change this to approximator
                changeTime = changeTime + (int)GetRandomNumber(1, 160);
                desiredTheta = GetRandomNumber(-0.5, 0.5);
            }
        }

        public double GetRandomNumber(double minimum, double maximum)
        {
            return random.NextDouble() * (maximum - minimum) + minimum;
        }

        private double CalculateReward()
        {
            //TODO:: change reward maybe?
            double reward = -Math.Abs(theta - desiredTheta); // -dif^2

            return reward;
        }
    }
}
